import * as THREE from 'three';

export class FormationController {
  constructor() {
    this.mainDrone = {
      position: new THREE.Vector3(0, 3, 0),
      rotation: new THREE.Euler(0, 0, 0),
      velocity: new THREE.Vector3(0, 0, 0)
    };
    
    this.formationDrones = [];
    this.formationType = 'circle'; // circle, line, triangle, diamond
    this.formationRadius = 3;
    this.formationHeight = 0; // Height offset from main drone
    this.rotationSpeed = 0.5; // Formation rotation speed
    this.currentRotation = 0;
    this.isFormationActive = false;
    
    // Program functions
    this.programs = {
      orbit: {
        name: 'Orbital Dance',
        description: 'Drones orbit around the main drone in a circular pattern',
        active: false,
        speed: 1,
        radius: 4,
        heightVariation: 1
      },
      spiral: {
        name: 'Spiral Ascent',
        description: 'Drones spiral up and down around the main drone',
        active: false,
        speed: 0.8,
        radius: 3,
        heightRange: 2
      }
    };
    
    this.activeProgram = null;
    this.programTime = 0;
    
    this.initializeFormation();
  }

  initializeFormation(droneCount = 4) {
    this.formationDrones = [];
    
    for (let i = 0; i < droneCount; i++) {
      const angle = (i / droneCount) * Math.PI * 2;
      const drone = {
        id: i,
        position: new THREE.Vector3(
          Math.cos(angle) * this.formationRadius,
          this.mainDrone.position.y + this.formationHeight,
          Math.sin(angle) * this.formationRadius
        ),
        rotation: new THREE.Euler(0, angle, 0),
        velocity: new THREE.Vector3(0, 0, 0),
        targetPosition: new THREE.Vector3(),
        baseAngle: angle,
        color: this.getDroneColor(i),
        trail: [] // For visual effects
      };
      
      this.formationDrones.push(drone);
    }
  }

  getDroneColor(index) {
    const colors = ['#e74c3c', '#3498db', '#2ecc71', '#f39c12', '#9b59b6', '#1abc9c'];
    return colors[index % colors.length];
  }

  // Program Function 1: Orbital Dance
  executeOrbitalDance(deltaTime) {
    if (!this.programs.orbit.active) return;
    
    this.programTime += deltaTime * this.programs.orbit.speed;
    
    this.formationDrones.forEach((drone, index) => {
      const baseAngle = drone.baseAngle;
      const orbitAngle = this.programTime + baseAngle;
      const heightOffset = Math.sin(this.programTime * 2 + index) * this.programs.orbit.heightVariation;
      
      // Calculate orbital position
      drone.targetPosition.set(
        this.mainDrone.position.x + Math.cos(orbitAngle) * this.programs.orbit.radius,
        this.mainDrone.position.y + heightOffset,
        this.mainDrone.position.z + Math.sin(orbitAngle) * this.programs.orbit.radius
      );
      
      // Add some variation for each drone
      const variation = Math.sin(this.programTime * 3 + index * 0.5) * 0.5;
      drone.targetPosition.x += variation;
      drone.targetPosition.z += variation;
      
      // Smooth movement towards target
      drone.position.lerp(drone.targetPosition, deltaTime * 2);
      
      // Face the center
      const lookDirection = this.mainDrone.position.clone().sub(drone.position);
      drone.rotation.y = Math.atan2(lookDirection.x, lookDirection.z);
    });
  }

  // Program Function 2: Spiral Ascent
  executeSpiralAscent(deltaTime) {
    if (!this.programs.spiral.active) return;
    
    this.programTime += deltaTime * this.programs.spiral.speed;
    
    this.formationDrones.forEach((drone, index) => {
      const baseAngle = drone.baseAngle;
      const spiralAngle = this.programTime * 2 + baseAngle;
      const heightCycle = Math.sin(this.programTime + index * 0.8) * this.programs.spiral.heightRange;
      const radiusVariation = Math.cos(this.programTime * 1.5 + index) * 0.5;
      
      // Calculate spiral position
      drone.targetPosition.set(
        this.mainDrone.position.x + Math.cos(spiralAngle) * (this.programs.spiral.radius + radiusVariation),
        this.mainDrone.position.y + heightCycle,
        this.mainDrone.position.z + Math.sin(spiralAngle) * (this.programs.spiral.radius + radiusVariation)
      );
      
      // Add vertical spiral motion
      const verticalSpiral = Math.sin(this.programTime * 4 + index * 1.2) * 0.3;
      drone.targetPosition.y += verticalSpiral;
      
      // Smooth movement towards target
      drone.position.lerp(drone.targetPosition, deltaTime * 1.5);
      
      // Dynamic rotation based on movement
      const velocity = drone.targetPosition.clone().sub(drone.position);
      if (velocity.length() > 0.01) {
        drone.rotation.y = Math.atan2(velocity.x, velocity.z);
        drone.rotation.z = velocity.x * 0.3; // Banking effect
      }
    });
  }

  // Standard formation patterns
  updateFormation(deltaTime) {
    if (!this.isFormationActive) return;
    
    this.currentRotation += deltaTime * this.rotationSpeed;
    
    this.formationDrones.forEach((drone, index) => {
      let targetPos = new THREE.Vector3();
      
      switch (this.formationType) {
        case 'circle':
          const angle = drone.baseAngle + this.currentRotation;
          targetPos.set(
            this.mainDrone.position.x + Math.cos(angle) * this.formationRadius,
            this.mainDrone.position.y + this.formationHeight,
            this.mainDrone.position.z + Math.sin(angle) * this.formationRadius
          );
          break;
          
        case 'line':
          const spacing = this.formationRadius / 2;
          targetPos.set(
            this.mainDrone.position.x + (index - this.formationDrones.length / 2) * spacing,
            this.mainDrone.position.y + this.formationHeight,
            this.mainDrone.position.z - this.formationRadius
          );
          break;
          
        case 'triangle':
          if (index === 0) {
            targetPos.set(this.mainDrone.position.x, this.mainDrone.position.y + this.formationHeight, this.mainDrone.position.z - this.formationRadius);
          } else {
            const triAngle = ((index - 1) / (this.formationDrones.length - 1)) * Math.PI + Math.PI;
            targetPos.set(
              this.mainDrone.position.x + Math.cos(triAngle) * this.formationRadius * 0.7,
              this.mainDrone.position.y + this.formationHeight,
              this.mainDrone.position.z + Math.sin(triAngle) * this.formationRadius * 0.7
            );
          }
          break;
          
        case 'diamond':
          const diamondAngle = (index / this.formationDrones.length) * Math.PI * 2 + this.currentRotation;
          const diamondRadius = index % 2 === 0 ? this.formationRadius : this.formationRadius * 0.6;
          targetPos.set(
            this.mainDrone.position.x + Math.cos(diamondAngle) * diamondRadius,
            this.mainDrone.position.y + this.formationHeight + (index % 2 === 0 ? 0.5 : -0.5),
            this.mainDrone.position.z + Math.sin(diamondAngle) * diamondRadius
          );
          break;
      }
      
      drone.targetPosition.copy(targetPos);
      drone.position.lerp(drone.targetPosition, deltaTime * 3);
      
      // Face forward or towards center
      if (this.formationType === 'line') {
        drone.rotation.y = this.mainDrone.rotation.y;
      } else {
        const lookDirection = this.mainDrone.position.clone().sub(drone.position);
        drone.rotation.y = Math.atan2(lookDirection.x, lookDirection.z);
      }
    });
  }

  update(deltaTime) {
    // Update main drone (this would be controlled by the existing FlightController)
    
    // Execute active program
    if (this.activeProgram) {
      switch (this.activeProgram) {
        case 'orbit':
          this.executeOrbitalDance(deltaTime);
          break;
        case 'spiral':
          this.executeSpiralAscent(deltaTime);
          break;
      }
    } else {
      // Standard formation behavior
      this.updateFormation(deltaTime);
    }
    
    // Update trails for visual effects
    this.updateTrails();
  }

  updateTrails() {
    this.formationDrones.forEach(drone => {
      drone.trail.push(drone.position.clone());
      if (drone.trail.length > 20) {
        drone.trail.shift();
      }
    });
  }

  // Control methods
  setFormationType(type) {
    this.formationType = type;
    this.currentRotation = 0;
  }

  setFormationRadius(radius) {
    this.formationRadius = Math.max(1, Math.min(10, radius));
  }

  setFormationHeight(height) {
    this.formationHeight = Math.max(-3, Math.min(3, height));
  }

  activateProgram(programName) {
    // Deactivate all programs
    Object.keys(this.programs).forEach(key => {
      this.programs[key].active = false;
    });
    
    if (programName && this.programs[programName]) {
      this.programs[programName].active = true;
      this.activeProgram = programName;
      this.programTime = 0;
      this.isFormationActive = false;
    } else {
      this.activeProgram = null;
      this.isFormationActive = true;
    }
  }

  toggleFormation() {
    this.isFormationActive = !this.isFormationActive;
    if (!this.isFormationActive) {
      this.activeProgram = null;
      Object.keys(this.programs).forEach(key => {
        this.programs[key].active = false;
      });
    }
  }

  updateMainDronePosition(position, rotation) {
    this.mainDrone.position.copy(position);
    this.mainDrone.rotation.copy(rotation);
  }

  getFormationState() {
    return {
      mainDrone: this.mainDrone,
      formationDrones: this.formationDrones,
      formationType: this.formationType,
      formationRadius: this.formationRadius,
      formationHeight: this.formationHeight,
      isFormationActive: this.isFormationActive,
      activeProgram: this.activeProgram,
      programs: this.programs,
      rotationSpeed: this.rotationSpeed
    };
  }
}
